//
// Created by infore on 2024/6/25.
//
#include <iostream>
#include "move_planner.h"
#include <cmath>

void record(StepType axis, HisStep& hisStep) {

    if (hisStep.cur == axis) {
        hisStep.hisMap[axis]++;
    }else {
        hisStep.hisMap[hisStep.cur] = 0;
        hisStep.hisMap[axis]++;
    }
    hisStep.cur = axis;
};


bool getX(Step& oriStep, HisStep& hisStep, Step& step, float thetathrehold) {
    bool flag = true;
    if (abs(oriStep.x) > thetathrehold and hisStep.hisMap[X] < 3) {
        step = Step(oriStep.x,0.0, 0.0);
        record(X, hisStep);

    }else {
        flag = false;
    }
    return flag;
}

bool getY(Step& oriStep, HisStep& hisStep, Step& step, float thetathrehold) {
    bool flag = true;
    if (abs(oriStep.y) > thetathrehold and hisStep.hisMap[Y] < 3) {
        step.y = oriStep.y;
        step = Step(0.0,oriStep.y, 0.0);
        record(Y, hisStep);
    }else {
        flag = false;
    }
    return flag;
}
bool getTHETA(Step& oriStep, HisStep& hisStep, Step& step, float thetathrehold) {
    bool flag = true;
    if (abs(oriStep.theta) > thetathrehold and hisStep.cur == THETA
        and hisStep.hisMap[THETA] < 3) {
        step = Step(0.0,0.0, oriStep.theta);
        record(THETA, hisStep);
    }else {
        flag = false;
    }
    return flag;
}



Step getStep(float x, float y, float theta, HisStep& hisStep) {
    Step step = Step(x= x, y= y, theta= theta);
    Step ret;
//    vector<string> orders = {"THETA", "Y", "X"};
//    粗调整
    if(getTHETA(step, hisStep, ret, TOLERATE_THETA_1)) {
        return ret;
    }else if(getY(step, hisStep, ret, TOLERATE_Y_1)){
        return ret;
    }else if (getX(step, hisStep, ret, TOLERATE_X_1)) {
        return ret;
    }
    //精细调整
    else if(getTHETA(step, hisStep, ret, TOLERATE_THETA_2)) {
        return ret;
    }else if(getY(step, hisStep, ret, TOLERATE_Y_2)) {
        return ret;
    }else if(getX(step, hisStep, ret, TOLERATE_X_2)) {
        return ret;
    }else if (step.x < TOLERATE_X_2 and step.y < TOLERATE_Y_2 and step.theta < TOLERATE_THETA_2) {
        ret.status = StepStatus::SUCCESS;
        return ret;
    }else {
        cout << "------missing------" << endl;
        return ret;
    };




    return ret;
}

